.syntax unified
.arm
.align 2

.equ user_mode    , 0x10
.equ fiq_mode     , 0x11
.equ irq_mode     , 0x12
.equ svc_mode     , 0x13
.equ moni_mode    , 0x16
.equ abort_mode   , 0x17
.equ undef_mode   , 0x1b
.equ sys_mode     , 0x1f

.equ sys_heap_size, 0x80000 /* 512K */

.section .text
int_disable:
    cpsid i
    bx    lr

int_enable:
    cpsie i
    bx    lr

.global irq_handler
irq_handler:
    subs  lr,   lr, #4
    stmdb sp!,  {r0-r12, lr}

    ldr   r1,   =irq_nest_count
    ldr   r0,   [r1]
    adds  r0,   r0, #0x01
    str   r0,   [r1]

    mrs   r0,   spsr
    push  {r0}

    cps   #svc_mode
    /* ack irq */
    mrc   p15,  4, r1, c15, c0, 0
    adds  r1,   r1, #0x2000
    adds  r1,   r1, #0x0C
    ldr   r0,   [r1]
    
    push  {r14}
    cpsie i
    bl    arch_irq_handler
    cpsid i
    pop   {r14}

    cps   #irq_mode

    ldr   r1,   =irq_nest_count
    ldr   r0,   [r1]
    subs  r0,   r0, #0x01
    str   r0,   [r1]

    /* if no irq, switch context */
    cmp   r0,   0
    beq   arch_context_switch_from_irq

    pop   {r0}
    msr   spsr, r0
    pop   {r0-r12, lr}

    /* else return */
    movs  pc, lr
    
.global context_switch
arch_context_switch_from_irq:
    pop   {r0}
    msr   spsr, r0
    pop   {r0-r12, lr}

arch_context_save:
    srsdb sp!,  #sys_mode
    cps   #sys_mode
    push  {r0-r12, lr}

    ldr   r0,   =g_cur_ctx
    /* now r1 point to last context */
    ldr   r1,   [r0]        
    adds  r1,   r1, #4
    str   sp,   [r1]

context_update:
    bl context_update_impl

arch_context_restore:
    cps   #sys_mode

    ldr   r1,   =g_cur_ctx
    /* now r0 point to next context */
    ldr   r0,   [r1]
    adds  r0,   r0, #4
    ldr   sp,   [r0]

    pop   {r0-r12, lr}
    rfeia sp!

.global swi_handler
swi_handler:
    /* start first context here */
    b     arch_context_restore

.global arch_start_first_context
arch_start_first_context:
    svc   #0

.section .bss
sys_heap_base:
.space sys_heap_size
sys_heap_end:


